

/*-------------------------------------------------------
                 Headerfiles 
---------------------------------------------------------*/
//#include <REG167.H>
#include "Decoder.h"

extern int write_mem(char *pdst, char *psrc, int len);
extern int read_mem(char *pdst);
extern void NewProtokollPart(char* Buf, int AnzByte);


void Decode(char* buf)
{
	int bufPos = 0;

	char length = 0;
	char* srcAdr = 0;
	char* destAdr = 0;
	int readValue = 0;
	int iReading = 0;
	int errorFlag = 0;
	
	// Reset Values
	retPos = 0;
	
	switch(buf[0] & 0x0F) { // Function Select, lower byte
	
		// COM TEST
		case 1:
			if(buf[1] == (char)0xAA && buf[2] == (char)0x55) {
				// Send Response
				retBuf[retPos] = 0x55;
				retPos += 1;
				
				retBuf[retPos] = (char)0xAA;
				retPos += 1;
				
			}
			else {
			
			}
		
		break;
		
		// READ
		case 2:
			bufPos = 1;
		
			// get srcAddress
			srcAdr = &buf[bufPos];
			bufPos += 3;
		
			// get Length
			length = buf[bufPos];
			
			prepareResponseHeader(srcAdr, length);
			
			// Reading Loop

			for(iReading = 0; iReading < length; iReading++) {
				// Call Read Memory Fnc
				//readValue = readmem(srcAdr + iReading);  // TODO: check for signed/unsigned!
				readValue = read_mem(srcAdr + iReading);  // TODO: check for signed/unsigned!
				if(readValue < 256) {
					// OK
					retBuf[retPos] = (char)readValue;
					retPos += 1;
				}
				else {
					errorFlag = 1;
				
				}
			}
			
			// Send Response 1
			if(errorFlag == 1) { // ERROR
				// TODO
			}
			else {	// OK
				//newTx(retBuf, retPos);
				NewProtokollPart(retBuf, retPos);
			}
		
		break;
		
		// WRITE
		case 3:
			bufPos = 1;
		
			// get Dest Adr
			destAdr = &buf[bufPos];
			bufPos += 3;
		
			// get Length
			length = (char)&buf[bufPos];
			bufPos += 1;
			
			// get Src Adr = Pointer pos of DataArray
			srcAdr = &buf[bufPos];
			
			// Call write mem
			//if( writeMem(destAdr,srcAdr,length) < 256) {
			if( write_mem(destAdr,srcAdr,length) < 256) {
				// OK
				prepareResponseHeader(destAdr, length);	
				
				// Send Response
				//newTx(retBuf, retPos);
				NewProtokollPart(retBuf, retPos);
			
			}
			else {
			// ERROR
			
			}
		
		break;
		
		// ERROR
		default:
		
		break;
	}
}

void prepareResponseHeader(char* startAdr, int length) {
			
			// Prepare Response : StartAddress & Length
			retBuf[retPos++] = (char)&startAdr;			
			retBuf[retPos++] = (char)&(startAdr +1);
			retBuf[retPos++] = (char)&(startAdr +2);
			
			retBuf[retPos++] = (char)length;

}
